#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>
#include <chrono>
#include <random>

using namespace std::chrono_literals;

class CameraNode : public rclcpp::Node
{
public:
    CameraNode() : Node("camera_node"), frame_count_(0)
    {
        // 创建图像发布者，话题为/camera/image_raw，队列大小为10
        image_publisher_ = this->create_publisher<sensor_msgs::msg::Image>("camera/image_raw", 10);
        
        // 设置定时器，每100ms发布一次图像（10Hz）
        timer_ = this->create_wall_timer(
            100ms, 
            std::bind(&CameraNode::publish_image, this)
        );
        
        RCLCPP_INFO(this->get_logger(), "Camera node initialized");
    }

private:
    void publish_image()
    {
        try {
            // 生成模拟图像
            cv::Mat image = generate_simulated_image();
            
            // 将OpenCV图像转换为ROS Image消息（添加错误处理）
            cv_bridge::CvImage cv_image;
            cv_image.header.stamp = this->get_clock()->now();
            cv_image.header.frame_id = "camera_link";
            cv_image.encoding = "bgr8";  // OpenCV默认是BGR格式
            cv_image.image = image;
            
            // 转换并发布消息
            sensor_msgs::msg::Image::SharedPtr msg = cv_image.toImageMsg();
            image_publisher_->publish(*msg);
            
            // 每10帧打印一次日志（包含时间戳，方便同步调试）
            if (frame_count_ % 10 == 0) {
                RCLCPP_INFO(this->get_logger(), "Published %d images. Timestamp: %d.%09d",
                    frame_count_,
                    msg->header.stamp.sec,
                    msg->header.stamp.nanosec);
            }
            
            frame_count_++;
        } catch (cv_bridge::Exception& e) {
            // 捕获转换错误（关键排查点）
            RCLCPP_ERROR(this->get_logger(), "cv_bridge conversion error: %s", e.what());
        } catch (std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Error publishing image: %s", e.what());
        }
    }
    
    cv::Mat generate_simulated_image()
    {
        // 创建640x480的浅灰色背景图像
        cv::Mat image(480, 640, CV_8UC3, cv::Scalar(240, 240, 240));
        
        // 绘制移动的矩形（模拟障碍物）
        int x = frame_count_ % 540;  // 让矩形在水平方向移动（0~539像素）
        cv::rectangle(
            image, 
            cv::Point(x, 200),          // 左上角坐标
            cv::Point(x + 100, 300),    // 右下角坐标（宽100，高100像素）
            cv::Scalar(0, 255, 0),      // 绿色 (OpenCV中是BGR格式)
            -1                          // 填充矩形
        );
        
        // 绘制网格（模拟地面纹理）
        for (int i = 0; i < 640; i += 40) {
            cv::line(image, cv::Point(i, 0), cv::Point(i, 480), cv::Scalar(200, 200, 200), 1);
        }
        for (int i = 0; i < 480; i += 40) {
            cv::line(image, cv::Point(0, i), cv::Point(640, i), cv::Scalar(200, 200, 200), 1);
        }
        
        // // 额外添加文字标注（方便确认图像是否正确生成）
        // cv::putText(
        //     image, 
        //     "Frame: " + std::to_string(frame_count_), 
        //     cv::Point(10, 30), 
        //     cv::FONT_HERSHEY_SIMPLEX, 
        //     1.0, 
        //     cv::Scalar(0, 0, 255),  // 红色文字
        //     2
        // );
        
        return image;
    }
    
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    int frame_count_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CameraNode>());
    rclcpp::shutdown();
    return 0;
}
    